import rtde_control
import rtde_receive

ip="192.168.3.140"
rtde_c=rtde_control.RTDEControlInterface(ip)
rtde_r=rtde_receive.RTDEReceiveInterface(ip)
# grap_pose=[0.6872853048650824, -0.1308957273597933, 0.17394818223574388, 6.256728706533603e-05, -3.141092053034699, 0.0001309061484982937]  #v2
# grap_pose=[0.6874792884908796,-0.1290049028269131,0.17394818223574388,0.00014219014866164234,-3.1375895073275664,2.166236079437557e-05]#v3
grap_pose = [0.47745133600881506, -0.0871746888476016, 0.1519502484440497, 0.008667017107691103, -3.127866260952955, 0.011422669020294273]#new position
# grap_pose=[0.6874965719790044,
# -0.1224151416325757,
# 0.11623619518163042,
# 0.00016992408087868394,
# -3.1375989597358496,
# 0.0003549488611648718]

# grap_pose=[0.6861132884039939,-0.12243507121158939,0.11622008563921185,0.0001450022568494896,-3.137480067901388,0.0004328617878218785]#复现two-stage
# grap_pose[2]+=0.01

# grap_pose = [0.6874524902679658, -0.12907513503568732, 0.1543933712663551, -0.00012414404159705842, -3.13757622734271, 3.9352716908090705e-05]#坤维传感器后
# grap_pose[2]+=0.01
rtde_c.moveL(grap_pose,0.2,0.05)
# print(rtde_r.getActualTCPPose())

